#ifndef GAMEENGINE_GEOMETRY_QUATERNION_HPP
#define GAMEENGINE_GEOMETRY_QUATERNION_HPP

#include <math.h>

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================
// Implementation of the Quaternion class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Quaternion<T>::Quaternion() {}

//! Constructor from three coordinates
/*!
 @param x The first coordinate
 @param y The second coordinate
 @param z The third coordinate
 @param w The fourth coordinate
 @note Class T should have a default constructor
 @note Class T should have an assignment operator
 */
template <class T>
Quaternion<T>::Quaternion(const T& x, const T& y, const T& z, const T& w) {
	coordinates_[0] = x;
	coordinates_[1] = y;
	coordinates_[2] = z;
	coordinates_[3] = w;
}

//! Copy constructor
/*!
 @param rhs The point to copy
 @note Class T should have a default constructor
 @note Class T should have an assignment operator
 */
template <class T>
Quaternion<T>::Quaternion(const Quaternion& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] = rhs.coordinates_[k];
	}
}

//! Copy constructor
/*!
 @param rhs The point to copy
 @note Class T should have a default constructor
 @note Class T should have an assignment operator
 */
template <class T>
template <class R>
Quaternion<T>::Quaternion(const Quaternion<R>& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] = (T)rhs[(Quaternions::Index)k];
	}
}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Quaternion<T>::~Quaternion() {}

//! Assignment operator
/*!
 @param rhs The quaternion to copy
 @note Class T should have an assignment operator
 */
template <class T>
Quaternion<T>& Quaternion<T>::operator=(const Quaternion& rhs) {
	if ( &rhs != this ) {
		for ( int k = 0; k < 4; ++k ) {
			coordinates_[k] = rhs.coordinates_[k];
		}
	}
	return *this;
}

//! Assignment operator
/*!
 @param rhs The quaternion to copy
 @note Class T should have an assignment operator
 @note Class R should have implicit conversion with class T
 */
template <class T>
template <class R>
Quaternion<T>& Quaternion<T>::operator=(const Quaternion<R>& rhs) {
	if ( &rhs != this ) {
		for ( int k = 0; k < 4; ++k ) {
			coordinates_[k] = (T)rhs[(Quaternions::Index)k];
		}
	}
	return *this;
}

//! Addition operator
/*!
 @param rhs The second quaternion to use for addition
 */
template <class T>
Quaternion<T> Quaternion<T>::operator+(const Quaternion<T>& rhs) {
	return Quaternion<T>(*this).operator+=(rhs);
}

//! Addition operator
/*!
 @param rhs The second quaternion to use for addition
 */
template <class T>
template <class R>
Quaternion<T> Quaternion<T>::operator+(const Quaternion<R>& rhs) {
	return Quaternion<T>(*this).operator+=(rhs);
}

//! Addition operator
/*!
 @param rhs The quaternion to use for addition
 @note Class T should have an addition operator
 */
template <class T>
Quaternion<T>& Quaternion<T>::operator+=(const Quaternion& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] += rhs.coordinates_[k];
	}
	return *this;
}

//! Addition operator
/*!
 @param rhs The quaternion to use for addition
 @note Class T should have an addition operator
 @note Class R should have implicit conversion with class T
 */
template <class T>
template <class R>
Quaternion<T>& Quaternion<T>::operator+=(const Quaternion<R>& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] += rhs[(Quaternions::Index)k];
	}
	return *this;
}

//! Subtraction operator
/*!
 @param rhs The second quaternion to use for subtraction
 */
template <class T>
Quaternion<T> Quaternion<T>::operator-(const Quaternion<T>& rhs) {
	return Quaternion<T>(*this).operator-=(rhs);
}

//! Subtraction operator
/*!
 @param rhs The second quaternion to use for subtraction
 */
template <class T>
template <class R>
Quaternion<T> Quaternion<T>::operator-(const Quaternion<R>& rhs) {
	return Quaternion<T>(*this).operator-=(rhs);
}

//! Subtraction operator
/*!
 @param rhs The quaternion to use for subtraction
 @note Class T should have a subtraction operator
 */
template <class T>
Quaternion<T>& Quaternion<T>::operator-=(const Quaternion& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] -= rhs.coordinates_[k];
	}
	return *this;
}

//! Subtraction operator
/*!
 @param rhs The quaternion to use for subtraction
 @note Class T should have a subtraction operator
 @note Class R should have implicit conversion with class T
 */
template <class T>
template <class R>
Quaternion<T>& Quaternion<T>::operator-=(const Quaternion<R>& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] -= rhs[(Quaternions::Index)k];
	}
	return *this;
}

//! Multiplication operator
/*!
 @param rhs The second quaternion to use for multiplication
 */
template <class T>
Quaternion<T> Quaternion<T>::operator*(const Quaternion<T>& rhs) {
	return Quaternion<T>(*this).operator*=(rhs);
}

//! Multiplication operator
/*!
 @param rhs The second quaternion to use for multiplication
 */
template <class T>
template <class R>
Quaternion<T> Quaternion<T>::operator*(const Quaternion<R>& rhs) {
	return Quaternion<T>(*this).operator*=(rhs);
}

//! Multiplication operator
/*!
 @param rhs The quaternion to use for multiplication
 @note Class T should have an assignment operator
 */
template <class T>
Quaternion<T>& Quaternion<T>::operator*=(const Quaternion& rhs) {
	// Copy local coordinates
	T a0 = coordinates_[0];
	T b0 = coordinates_[1];
	T c0 = coordinates_[2];
	T d0 = coordinates_[3];

	// Update coordinates
	coordinates_[0] = rhs[Quaternions::First]*b0 + a0*rhs[Quaternions::Second] - rhs[Quaternions::Third]*d0 + c0*rhs[Quaternions::Fourth];
	coordinates_[1] = rhs[Quaternions::First]*c0 + a0*rhs[Quaternions::Third] + rhs[Quaternions::Second]*d0 - b0*rhs[Quaternions::Fourth];
	coordinates_[2] = -rhs[Quaternions::Second]*c0 + b0*rhs[Quaternions::Third] + rhs[Quaternions::First]*d0 + a0*rhs[Quaternions::Fourth];
	coordinates_[3] = a0*rhs[Quaternions::First] - b0*rhs[Quaternions::Second] - c0*rhs[Quaternions::Third] - d0*rhs[Quaternions::Fourth];
	return *this;
}

//! Multiplication operator
/*!
 @param rhs The quaternion to use for multiplication
 @note Class T should have an assignment operator
 @note Class R should have implicit conversion with class T
 */
template <class T>
template <class R>
Quaternion<T>& Quaternion<T>::operator*=(const Quaternion<R>& rhs) {
	// Copy local coordinates
	T a0 = coordinates_[0];
	T b0 = coordinates_[1];
	T c0 = coordinates_[2];
	T d0 = coordinates_[3];

	// Update coordinates
	coordinates_[0] = (T)rhs[Quaternions::First]*b0 + a0*(T)rhs[Quaternions::Second] - (T)rhs[Quaternions::Third]*d0 + c0*(T)rhs[Quaternions::Fourth];
	coordinates_[1] = (T)rhs[Quaternions::First]*c0 + a0*(T)rhs[Quaternions::Third] + (T)rhs[Quaternions::Second]*d0 - b0*(T)rhs[Quaternions::Fourth];
	coordinates_[2] = -(T)rhs[Quaternions::Second]*c0 + b0*(T)rhs[Quaternions::Third] + (T)rhs[Quaternions::First]*d0 + a0*(T)rhs[Quaternions::Fourth];
	coordinates_[3] = a0*(T)rhs[Quaternions::First] - b0*(T)rhs[Quaternions::Second] - c0*(T)rhs[Quaternions::Third] - d0*(T)rhs[Quaternions::Fourth];
	return *this;
}

//! Multiplication operator
/*!
 @param rhs The value to use for multiplication
 @note Class T should have a multiplication operator
 */
template <class T>
Quaternion<T> Quaternion<T>::operator*(const T& rhs) {
	return Quaternion<T>(*this).operator*=(rhs);
}

//! Multiplication operator
/*!
 @param rhs The value to use for multiplication
 @note Class T should have a multiplication operator
 */
template <class T>
Quaternion<T>& Quaternion<T>::operator*=(const T& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] *= rhs;
	}
	return *this;
}

//! Division operator
/*!
 @param rhs The value to use for division
 @note Class T should have a division operator
 */
template <class T>
Quaternion<T> Quaternion<T>::operator/(const T& rhs) {
	return Quaternion<T>(*this).operator/=(rhs);
}

//! Division operator
/*!
 @param rhs The value to use for division
 @note Class T should have a division operator
 */
template <class T>
Quaternion<T>& Quaternion<T>::operator/=(const T& rhs) {
	for ( int k = 0; k < 4; ++k ) {
		coordinates_[k] /= rhs;
	}
	return *this;
}

//! Square length of the quaternion
/*!
 @return The square length of the quaternion
 */
template <class T>
T Quaternion<T>::square_length() const {
	return coordinates_[0]*coordinates_[0] + coordinates_[1]*coordinates_[1]
		+ coordinates_[2]*coordinates_[2] + coordinates_[3]*coordinates_[3];
}

//! Length of the quaternion
/*!
 @return The length of the quaternion
 */
template <class T>
T Quaternion<T>::length() const {
	return sqrt( square_length() );
}

//! Normalize the quaternion
/*!
 @return A reference to this quaternion after normalization
 */
template <class T>
Quaternion<T>& Quaternion<T>::normalize() {
	// Get length
	T len = length();

	// Update coordinates
	return (*this) /= len;
}

//! Normalize the quaternion
/*!
 @return A normalized version of this quaternion
 */
template <class T>
Quaternion<T> Quaternion<T>::normalize(const Quaternion& rhs) {
	return Quaternion<T>(*this).normalize();
}

//! Convert quaternion to a rotation matrix
template <class T>
Transformer3dOrientation<T> Quaternion<T>::to_rotation_matrix() const {
	// Get length
	T len = length(); len = (T)1.0 / (len*len);

	// Fill matrix
	Transformer3dOrientation<T> res;
	res.matrix_[0][0] = ( coordinates_[0]*coordinates_[0] + coordinates_[1]*coordinates_[1] - coordinates_[2]*coordinates_[2]
								- coordinates_[3]*coordinates_[3] ) * len;
	res.matrix_[1][0] = ( (T)2.0*coordinates_[1]*coordinates_[2] - (T)2.0*coordinates_[0]*coordinates_[3] ) * len;
	res.matrix_[2][0] = ( (T)2.0*coordinates_[0]*coordinates_[2] + (T)2.0*coordinates_[1]*coordinates_[3] ) * len;
	res.matrix_[0][1] = ( (T)2.0*coordinates_[0]*coordinates_[3] + (T)2.0*coordinates_[1]*coordinates_[2] ) * len;
	res.matrix_[1][1] = ( coordinates_[0]*coordinates_[0] - coordinates_[1]*coordinates_[1] + coordinates_[2]*coordinates_[2]
								- coordinates_[3]*coordinates_[3] ) * len;
	res.matrix_[2][1] = ( (T)2.0*coordinates_[2]*coordinates_[3] - (T)2.0*coordinates_[0]*coordinates_[1] ) * len;
	res.matrix_[0][2] = ( (T)2.0*coordinates_[1]*coordinates_[3] - (T)2.0*coordinates_[0]*coordinates_[2] ) * len;
	res.matrix_[1][2] = ( (T)2.0*coordinates_[0]*coordinates_[1] + (T)2.0*coordinates_[2]*coordinates_[3] ) * len;
	res.matrix_[2][2] = ( coordinates_[0]*coordinates_[0] - coordinates_[1]*coordinates_[1] - coordinates_[2]*coordinates_[2]
								+ coordinates_[3]*coordinates_[3] ) * len;
	return res;
}

//! Create quaternion from an angle and a rotation intensity around this angle
template <class T>
template <class R, class S>
Quaternion<T> Quaternion<T>::from_vector(const Vector3d<S>& rhs, const R& angle) {
	S len = rhs.length();
	return Quaternion<T>(
		(T)(rhs[Point3D::First]/len),
		(T)(rhs[Point3D::Second]/len),
		(T)(rhs[Point3D::Third]/len),
		(T)cos( sin(-0.5*(T)angle) )
	);
}

//! Accessor to the first coordinate (read-write)
/*!
 @return A reference to the first coordinate
 */
template <class T>
T& Quaternion<T>::x() {
	return coordinates_[0];
}

//! Accessor to the first coordinate (read only)
/*!
 @return A constant reference to the first coordinate
 */
template <class T>
const T& Quaternion<T>::x() const {
	return coordinates_[0];
}

//! Accessor to the second coordinate (read-write)
/*!
 @return A reference to the second coordinate
 */
template <class T>
T& Quaternion<T>::y() {
	return coordinates_[1];
}

//! Accessor to the second coordinate (read only)
/*!
 @return A constant reference to the second coordinate
 */
template <class T>
const T& Quaternion<T>::y() const {
	return coordinates_[1];
}

//! Accessor to the third coordinate (read-write)
/*!
 @return A reference to the third coordinate
 */
template <class T>
T& Quaternion<T>::z() {
	return coordinates_[2];
}

//! Accessor to the third coordinate (read only)
/*!
 @return A constant reference to the third coordinate
 */
template <class T>
const T& Quaternion<T>::z() const {
	return coordinates_[2];
}

//! Accessor to the fourth coordinate (read-write)
/*!
 @return A reference to the fourth coordinate
 */
template <class T>
T& Quaternion<T>::w() {
	return coordinates_[3];
}

//! Accessor to the fourth coordinate (read only)
/*!
 @return A constant reference to the fourth coordinate
 */
template <class T>
const T& Quaternion<T>::w() const {
	return coordinates_[3];
}

//! Accessor to the given coordinate (read-write)
/*!
 @return A reference to the given coordinate
 */
template <class T>
T& Quaternion<T>::operator[](Quaternions::Index index) {
	return coordinates_[(int)index];
}

//! Accessor to the given coordinate (read only)
/*!
 @return A constant reference to the given coordinate
 */
template <class T>
const T& Quaternion<T>::operator[](Quaternions::Index index) const {
	return coordinates_[(int)index];
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
